#include "ros/ros.h"
#include "std_msgs/String.h"
#include <storage/eyeDistAction.h>
#include <storage/androidCall.h>
#include <actionlib/client/simple_action_client.h>
#include <iostream>
using namespace std;

typedef actionlib::SimpleActionClient<storage::eyeDistAction> Client;
void sendGoal(string imgID, string faceNumber, string eyeDist)
{
	ROS_ERROR("start");
	Client client("eyeDist",true);
	client.waitForServer();
	ROS_ERROR("end1");
	storage::eyeDistGoal goal;
	//imgID
	goal.target.push_back(imgID);
	//faceNumber
	goal.target.push_back(faceNumber);
	//eyeDist
	goal.target.push_back(eyeDist);
	ROS_ERROR("end2");
	client.sendGoal(goal);
	ROS_ERROR("end3");
	client.waitForResult(ros::Duration(5.0)); 
	if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
		printf("Yay! The dishes are now clean");
	printf("Current State: %s\n", client.getState().toString().c_str());
	ROS_ERROR("end4");
}

void processFaces(storage::androidCall msg)
{
	//do android stuff
	for(int i=0; i<msg.numFaces; i++){
		std::cout<<"looping";
		std::ostringstream facenum;
		facenum<<i;
		sendGoal(msg.imageID,facenum.str(),"19");
	}
}
int main(int argc, char** argv)
{
	ros::init(argc,argv,"storageClient2");
	std::cout << "init" << std::endl;
	ros::NodeHandle n;
	ros::Subscriber sub = n.subscribe("androidFace",100,processFaces);
	ros::Publisher pub = n.advertise<std_msgs::String>("android_request",1);
	ros::Subscriber sub2 = n.subscribe("android_response",100,androidCallback);
	ros::spin();
	return 0;
}
